/**
    Copyright 2015 Table Rider
    By: Richard Solti & Kristoffer Dahl
*/

#include "Motor.h"
#include "Definitions.h"

Motor::Motor(int l)
{
    left = l;
    this->period = PWM_PERIOD;

    if(left)
    {
        DUTY = (unsigned int*)&REG_PWM_CDTY6;
        ENCODER = (unsigned int*)&REG_TC0_CV0;
        CURRENT = (unsigned int*)&REG_ADC_CDR7;
        DIRECTION = LEFT_DIRECTION;
    }
    else
    {
        DUTY = (unsigned int*)&REG_PWM_CDTY7;
        ENCODER = (unsigned int*)&REG_TC2_CV0;
        CURRENT = (unsigned int*)&REG_ADC_CDR6;//MIGHT BE WRONG CHANNEL! A6
        DIRECTION = RIGHT_DIRECTION;
    }
}

Motor::~Motor()
{

}

void Motor::setDuty(int duty)
{
	duty = clamp(-period, duty, period);
	setDirection(duty);
	if(duty != 0){
		*DUTY = clamp(0,abs(duty),period);
	}
	else{
		*DUTY = 0;
	}
}
void Motor::setDirection(int dir)
{
	switch(DIRECTION){
		case RIGHT_DIRECTION:{
    			if(dir < 0)
        			REG_PIOD_CODR = DIRECTION;
    			else
        			REG_PIOD_SODR = DIRECTION;
				break;
		}
		case LEFT_DIRECTION:{
			if(dir < 0)
        			REG_PIOD_SODR = DIRECTION;
    			else
        			REG_PIOD_CODR = DIRECTION;
				break;
		}
	}
}

int Motor::getEncoder()
{
    return *ENCODER;//( left ? REG_TC0_CV0 : REG_TC2_CV0);
}

int Motor::getCurrent()
{
    return *CURRENT;
}

int Motor::getLeft(){
	return left;
}

double Motor::getRealCurrent()
{
    return (double)*CURRENT * 0.00161/4;
}
